
#include "stm32f10x.h" //stm32头文件
#include "usart.h"
#include "sys.h"
#include "delay.h"
#include "led.h"
#include "vl53l0x.h"
#include "vl53l0x_gen.h"
#include "OLED.h"
#include "pwm.h"
#include "pid.h"
#include "key.h"
#include "Timer.h"

//激光接线
//PA3 SCL //PA2 SDA //PA5 XSD
//OLED接线
//PB8 SCL //PB9 SDA
//驱动 pwm：PA1
//按键1：增加 PB1 按键2：确定 PB10 按键3：减少 PB11
/******************************************************************************/
extern VL53L0X_Dev_t vl53l0x_dev;//设备I2C数据参数
/******************************************************************************/
int ex_h=40;

float z;
float la_e,PWM_OUT,h;
/*
*	Mode1:小球置于圆管底部，启动后5 秒内控制小球向上到达BC段，并维持5秒以上。
*	Mode2:当小球维持在BC段时，用长形纸板(宽度为风机直径的三分之一)遮挡风机的进风，小球继续稳定维持在BC段。
*	Mode3:以C点的坐标为0cm、B点的坐标为15cm;用键盘设定小球的高度位置(单位:cm)，启动后使小球稳定地处于指定的高度3秒以上，上下波动不超过±1cm。
*	Mode4:小球置于圆管底部，启动后5秒内控制小球向上到达圆管顶部处A端，且不跳离，维持5秒以上(CCR=90的时候可以刚好到达)
*	Mode5:AB->CD->AB->CD->冲出
*/
#define BESTWISH 	960
#define MODE_NUM	5	//模式数量
#define DISTANCE	7.50//激光测距到管顶A距离
int Mode = 0;
int high;
int targetHigh;	
int flag = 1;
struct Xls
{
	int mid;
	int expect;
}xls[40];

void Xls_Init(void)
{
	//CD段
	xls[14].mid = 960;		xls[14].expect = 10;	
				  
	//BC段        
	xls[15].mid	= 960;		xls[15].expect	= 14;
				  
	xls[16].mid	= 960;		xls[16].expect	= 16;
				  
	xls[17].mid	= 960;		xls[17].expect	= 18;
				  
	xls[18].mid	= 960;		xls[18].expect	= 20;
				  
	xls[19].mid	= 960;		xls[19].expect	= 20;
				  
	xls[20].mid	= 960;		xls[20].expect	= 21;
				  
	xls[21].mid	= 960;		xls[21].expect	= 22;
				  
	xls[22].mid	= 960;		xls[22].expect	= 23;
				  
	xls[23].mid	= 960;		xls[23].expect	= 24;
				  
	xls[24].mid	= 960;		xls[24].expect	= 25;
										  
	xls[25].mid	= 960;		xls[25].expect	= 26;
										  
	xls[26].mid	= 960;		xls[26].expect	= 26;
										  
	xls[27].mid	= 960;		xls[27].expect	= 27;
										  
	xls[28].mid	= 960;		xls[28].expect	= 28;
										  
	xls[29].mid	= 960;		xls[29].expect	= 29;
	                        
	xls[30].mid	= 960;		xls[30].expect	= 30;
				  
	//AB段        
	xls[31].mid	= 960;		xls[31].expect	= 38;
}


int main(void)
{
	static char buf[VL53L0X_MAX_STRING_LENGTH];//测试模式字符串字符缓冲区
	VL53L0X_Error Status=VL53L0X_ERROR_NONE;//工作状态
	u8 mode=1; //0：默认；1：高精度；2：长距离；3：高速度
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//分组2
	delay_init();
	PWM_Init();
	Key_Init();
	Timer_Init();
	OLED_Init();
	//Xls_Init();
	if(vl53l0x_init(&vl53l0x_dev))     //vl53l0x初始化
	 {
//		printf("VL53L0X_Init Error!!!\r\n");
//		 OLED_ShowString(1,1,"Error");
//		delay_ms(200);
	 }
	
	if(vl53l0x_set_mode(&vl53l0x_dev,mode))   //配置测量模式
	{
//			printf("Mode Set Error!!!\r\n");
//			OLED_ShowString(2,1,"Error");
	}
	OLED_ShowString(2,1,"time:");
	OLED_ShowString(3,1,"high:");

	while(1)
		{	
		switch(Mode)
		{
			case 0:
				OLED_ShowString(1,1,"Waiting...");
				if(Key_GetDown())
					Mode++;
			break;
			case 1:
				OLED_ShowString(1,1,"Mode1     ");
				targetHigh = 15 + 7;
				flag = 1;
				PWM_SetCompare2(990);
				while(1)
				{
					Status = vl53l0x_start_single_test(&vl53l0x_dev,&vl53l0x_data,buf);//执行一次测量
					PWM_OUT = 0;
					high = 45 + DISTANCE - ((Distance_data)/10.0);
					OLED_ShowSignedNum(3,6,high,2);
					ex_h = targetHigh+2;
					z=Control_Algorithm(high,ex_h);
					PWM_OUT+=z;		
					PWM_OUT +=BESTWISH;
					PWM_SetCompare2(PWM_OUT);
					
					if((high>=15&&high<=30)&&flag)
					{
						Timer_Start();
						flag = 0;
					}
					
					OLED_ShowNum(2,6,Get_Num(),2);
					if(Key_GetUp())
					{
						Mode = 5;
						Timer_Finish();
						break;
					}
					else if(Key_GetDown())
					{
						Mode++;
						Timer_Finish();
						break;
					}
					else if(Key_GetEnter())
					{
						Timer_Finish();
						break;
					}
					
				}
			break;
			
			case 2:
				OLED_ShowString(1,1,"Mode2      ");
				targetHigh = 15 + 13;
				flag = 1;
				PWM_SetCompare2(990);
				while(1)
				{
					Status = vl53l0x_start_single_test(&vl53l0x_dev,&vl53l0x_data,buf);//执行一次测量
					PWM_OUT = 0;
					high = 45 + DISTANCE - ((Distance_data)/10.0);
					OLED_ShowSignedNum(3,6,high,2);
					ex_h = targetHigh+2;
					z=Control_Algorithm(high,ex_h);
					PWM_OUT+=z;		
					PWM_OUT +=BESTWISH;
					PWM_SetCompare2(PWM_OUT);
					
					if((high>=15&&high<=30)&&flag)
					{
						Timer_Start();
						flag = 0;
					}
					
					OLED_ShowNum(2,6,Get_Num(),2);
					if(Key_GetUp())
					{
						Mode--;
						Timer_Finish();
						break;
					}
					else if(Key_GetDown())
					{
						Mode++;
						Timer_Finish();
						break;
					}
					else if(Key_GetEnter())
					{
						Timer_Finish();
						break;
					}
				}
			break;
			
			case 3:
				PWM_SetCompare2(0);
				OLED_ShowString(1,1,"Mode3     ");
				OLED_ShowString(4,1,"targetHigh:");
				targetHigh = 0;
			
				OLED_ShowNum(4,12,targetHigh,2);
				while(1)
				{
					delay_ms(20);
					if(Key_GetDown())
					{
						targetHigh++;
						if(targetHigh>15)
							targetHigh = 0;
						OLED_ShowNum(4,12,targetHigh,2);
					}
					else if(Key_GetUp())
					{
						targetHigh--;
						if(targetHigh<0)
							targetHigh = 15;
						OLED_ShowNum(4,12,targetHigh,2);
					}
					else if(Key_GetEnter())
						break;
				}
				PWM_SetCompare2(990);
				targetHigh += 15;
				flag = 1;
				while(1)
				{    
					
					Status = vl53l0x_start_single_test(&vl53l0x_dev,&vl53l0x_data,buf);//执行一次测量
					PWM_OUT = 0;
					high = 45 + DISTANCE - ((Distance_data)/10.0);
					OLED_ShowSignedNum(3,6,high,2);
					ex_h = targetHigh+2+2;
					z=Control_Algorithm(high,ex_h);
					PWM_OUT+=z;		
					PWM_OUT +=BESTWISH;
					PWM_SetCompare2(PWM_OUT);
					
					if((targetHigh-high<=1 && targetHigh-high>=-1) && flag)
					{
						Timer_Start();
						flag = 0;
					}
					
					
					OLED_ShowNum(2,6,Get_Num(),2);
					if(Key_GetUp())
					{
						Mode--;
						Timer_Finish();
						OLED_ShowString(4,1,"                 ");
						break;
					}
					else if(Key_GetDown())
					{
						Mode++;
						Timer_Finish();
						OLED_ShowString(4,1,"                 ");
						break;
					}
					else if(Key_GetEnter())
					{
						Timer_Finish();
						break;
					}
				}
			break;
			
			case 4:
				OLED_ShowString(1,1,"Mode4     ");
				PWM_SetCompare2(2000);
				targetHigh = 47;
				while(targetHigh-high>1 || high-targetHigh>1)
				{
					Status = vl53l0x_start_single_test(&vl53l0x_dev,&vl53l0x_data,buf);//执行一次测量
					high = 45 + DISTANCE - ((Distance_data)/10.0);
					OLED_ShowNum(3,6,high,2);
				}
				Timer_Start();
				while(1)
				{
					OLED_ShowNum(2,6,Get_Num(),2);
					if(Key_GetUp())
					{
						Mode--;
						Timer_Finish();
						break;
					}
					else if(Key_GetDown())
					{
						Mode++;
						Timer_Finish();
						break;
					}
					else if(Key_GetEnter())
					{
						Timer_Finish();
						break;
					}
				}
			break;
			
			case 5:
				OLED_ShowString(1,1,"Mode5     ");
				int n = 2;
				while(n--)
				{
					//AB
					targetHigh =  7+15+15;
					flag = 1;
					while(1)
					{
						OLED_ShowNum(4,1,TIM2->CCR2,5);
						Status = vl53l0x_start_single_test(&vl53l0x_dev,&vl53l0x_data,buf);//执行一次测量
						PWM_OUT = 0;
						high = 45 + DISTANCE - ((Distance_data)/10.0);
						OLED_ShowSignedNum(3,6,high,2);
						ex_h = targetHigh+2;
						z=Control_Algorithm(high,ex_h);
						PWM_OUT+=z;		
						PWM_OUT +=BESTWISH;
						PWM_SetCompare2(PWM_OUT);
						
						if((high-targetHigh<=1&&high-targetHigh>=-1)&&flag)
						{
							Timer_Start();
							flag = 0;
						}
						
						OLED_ShowNum(3,6,high,2);
						OLED_ShowNum(2,6,Get_Num(),2);
						
						if(Get_Num()>=5)	
						{
							Timer_Finish();
							break;
						}
					}
					
					//CD
					targetHigh =  13;
					flag = 1;
					while(1)
					{
						OLED_ShowNum(4,1,TIM2->CCR2,5);
						Status = vl53l0x_start_single_test(&vl53l0x_dev,&vl53l0x_data,buf);//执行一次测量
						PWM_OUT = 0;
						high = 45 + DISTANCE - ((Distance_data)/10.0);
						OLED_ShowSignedNum(3,6,high,2);
						ex_h = targetHigh;
						z=Control_Algorithm(high,ex_h);
						PWM_OUT+=z;		
						PWM_OUT +=BESTWISH;
						PWM_SetCompare2(PWM_OUT);
						
						if((high<=15&&high>=0)&&flag)	
						{
							Timer_Start();
							flag = 0;
						}
						
						OLED_ShowNum(3,6,high,2);
						OLED_ShowNum(2,6,Get_Num(),2);
						
						if(Get_Num()>=5)
						{
							Timer_Finish();
							break;
						}
					}
					
				}
				
				//冲出桶外
				PWM_SetCompare2(7000);
				
				while(1)
				{
					if(Key_GetUp())
					{
						Mode--;
						break;
					}
					else if(Key_GetDown())
					{
						Mode = 1;
						break;
					}
					else if(Key_GetEnter())
					{
						break;
					}
				}
			break;
		}


	 }
}


/***************************************************************************/
/***************************************************************************/



